#include "Controller.h"
#include "Telescope.h"
#include "MeadeLX200.h"

void meade_lx200_start(Telescope *telescope) {
	struct termios options;
	speed_t baudrate = B9600;
	cfsetospeed(&options, (speed_t) baudrate);
	cfsetispeed(&options, (speed_t) baudrate);
	options.c_cflag &= ~PARENB;
	options.c_cflag &= ~CSTOPB;
	options.c_cflag &= ~CSIZE;
	options.c_cflag |= CS8;
	options.c_cflag |= CLOCAL | CREAD;
	options.c_cflag &= ~CRTSCTS;
	options.c_iflag &= ~(IXON|IXOFF|IXANY);
	options.c_lflag = 0;
	options.c_oflag = 0;
	options.c_cc[VTIME] = 1;
	options.c_cc[VMIN] = 60;
	tcsetattr(telescope->sdd, TCSANOW, &options);
}

void meade_lx200_execute_command(Telescope *telescope, Client *client, uint8_t command, char *params, char *callback) {
	char response[128];
	switch(command) {
		case GET_RA_DEC:
			meade_lx200_get_ra_dec(telescope, callback, response);
			network_send_response(client, response);
			break;
		case GOTO_RA_DEC:
			meade_lx200_goto_ra_dec(telescope, params, callback, response);
			network_send_response(client, response);
			break;
		default:
			Throw 500;
			break;
	}
}

void meade_lx200_get_ra_dec(Telescope *telescope, char *callback, char *response) {
	char data[4] = ":GR#", ra[32], dec[32];
	telescope_write_data(telescope, data);
	telescope_read_data(telescope, ra);
	strcpy(data, ":GD#");
	telescope_write_data(telescope, data);
	telescope_read_data(telescope, dec);
	sscanf(data, "%s,%s#", ra, dec);
	sprintf(response, "%s({\"ra\":\"%s\",\"dec\":\"%s\"})", callback, ra, dec);
}

void meade_lx200_goto_ra_dec(Telescope *telescope, char *params, char *callback, char *response) {
	char data[10], ra[32], dec[32];
	float hrs[3], dgs[3];
	sscanf(params, "%f+%f+%f+%f+%f+%f", &hrs[0], &hrs[1], &hrs[2], &dgs[0], &dgs[1], &dgs[2]);
	sprintf(ra, ":Sr%f:%f:%f#", hrs[0], hrs[1], hrs[2]);
	sprintf(dec, ":Sd%fº%f:%f#", dgs[0], dgs[1], dgs[2]);
	telescope_write_data(telescope, ra);
	telescope_read_data(telescope, data);
	telescope_write_data(telescope, dec);
	telescope_read_data(telescope, data);
	sprintf(response, "%s({\"%s\"})", callback, data);
}
